#include "st7789.h"
#include "math.h"

//Function to send command to the display
void ST7789_WriteCommand(uint8_t cmd) {
  DCN;
  CSN;
  HAL_SPI_Transmit(&ST7789_SPI_INSTANCE, &cmd, 1, HAL_MAX_DELAY);
  CS;
}

//Function to send data to the display
void ST7789_WriteByte(uint8_t data) {
  DC;
  CSN;
  HAL_SPI_Transmit(&ST7789_SPI_INSTANCE, &data, 1, HAL_MAX_DELAY);
  CS;
}

//Function to send multi-data to the display
void ST7789_WriteData(uint8_t *data, size_t data_size) {
  DC;
  CSN;
  HAL_SPI_Transmit(&ST7789_SPI_INSTANCE, data, data_size, HAL_MAX_DELAY);
  CS;
}

void ST7789_SetDisplayDirection(uint8_t direction) {
  ST7789_WriteCommand(0x36);
  ST7789_WriteByte(direction);
}

void ST7789_Init(void) {
  //Reset the display
  HAL_GPIO_WritePin(ST7789_RESET_GPIO_Port, ST7789_RESET_Pin, GPIO_PIN_SET);
  HAL_Delay(10);
  HAL_GPIO_WritePin(ST7789_RESET_GPIO_Port, ST7789_RESET_Pin, GPIO_PIN_RESET);
  HAL_Delay(10);
  HAL_GPIO_WritePin(ST7789_RESET_GPIO_Port, ST7789_RESET_Pin, GPIO_PIN_SET);
  HAL_Delay(10);

  //Initialization commands for ST7789
  ST7789_WriteCommand(0x01);  //Software reset
  HAL_Delay(50);
  ST7789_WriteCommand(0x11);  //Sleep out
  HAL_Delay(100);
  ST7789_WriteCommand(0x36);
  ST7789_WriteByte(0x00);
  ST7789_WriteCommand(0x3A);
  ST7789_WriteByte(0x05);
  ST7789_WriteCommand(0xB2);
  ST7789_WriteByte(0x0C);
  ST7789_WriteByte(0x0C);
  ST7789_WriteByte(0x00);
  ST7789_WriteByte(0x33);
  ST7789_WriteByte(0x33);
  ST7789_WriteCommand(0xB7);
  ST7789_WriteByte(0x35);
  ST7789_WriteCommand(0xBB);
  ST7789_WriteByte(0x19);
  ST7789_WriteCommand(0xC0);
  ST7789_WriteByte(0x2C);
  ST7789_WriteCommand(0xC2);
  ST7789_WriteByte(0x01);
  ST7789_WriteCommand(0xC3);
  ST7789_WriteByte(0x12);
  ST7789_WriteCommand(0xC4);
  ST7789_WriteByte(0x20);
  ST7789_WriteCommand(0xC6);
  ST7789_WriteByte(0x0F);
  ST7789_WriteCommand(0xD0);
  ST7789_WriteByte(0xA4);
  ST7789_WriteByte(0xA1);
  ST7789_WriteCommand(0xE0);
  ST7789_WriteByte(0xD0);
  ST7789_WriteByte(0x04);
  ST7789_WriteByte(0x0D);
  ST7789_WriteByte(0x11);
  ST7789_WriteByte(0x13);
  ST7789_WriteByte(0x2B);
  ST7789_WriteByte(0x3F);
  ST7789_WriteByte(0x54);
  ST7789_WriteByte(0x4C);
  ST7789_WriteByte(0x18);
  ST7789_WriteByte(0x0D);
  ST7789_WriteByte(0x0B);
  ST7789_WriteByte(0x1F);
  ST7789_WriteByte(0x23);
  ST7789_WriteCommand(0xE1);
  ST7789_WriteByte(0xD0);
  ST7789_WriteByte(0x04);
  ST7789_WriteByte(0x0C);
  ST7789_WriteByte(0x11);
  ST7789_WriteByte(0x13);
  ST7789_WriteByte(0x2C);
  ST7789_WriteByte(0x3F);
  ST7789_WriteByte(0x44);
  ST7789_WriteByte(0x51);
  ST7789_WriteByte(0x2F);
  ST7789_WriteByte(0x1F);
  ST7789_WriteByte(0x1F);
  ST7789_WriteCommand(0x13);  //Normal display mode on

  HAL_Delay(10);

  ST7789_SetDisplayDirection(ST7789_ROTATION);
  ST7789_FillScreen(ST7789_BLACK);

  ST7789_WriteCommand(0x29);  //Display on
}

void ST7789_SetAddressWindow(uint16_t x0, uint16_t y0, uint16_t x1, uint16_t y1) {
  x0 += ST7789_XSTART;
  y0 += ST7789_YSTART;

  x1 += ST7789_XSTART;
  y1 += ST7789_YSTART;

  ST7789_WriteCommand(0x2A);
  uint8_t data[] = {x0 >> 8, x0 & 0xFF, x1 >> 8, x1 & 0xFF};
  ST7789_WriteData(data, sizeof(data));

  ST7789_WriteCommand(0x2B);
  data[0] = y0 >> 8;
  data[1] = y0 & 0xFF;
  data[2] = y1 >> 8;
  data[3] = y1 & 0xFF;
  ST7789_WriteData(data, sizeof(data));
}

void ST7789_Pixel(uint16_t x, uint16_t y, uint16_t color) {
  ST7789_SetAddressWindow(x, y, x, y);
  ST7789_WriteCommand(0x2C);
  ST7789_WriteByte(color >> 8);
  ST7789_WriteByte(color & 0xFF);
}

void ST7789_String(uint16_t x, uint16_t y, char *str, uint16_t color, uint16_t bgColor, const FontDef *font) {
  uint8_t row, col;

  uint8_t buffer_color[2] = {color >> 8, color & 0xFF};
  uint8_t buffer_bg[2] = {bgColor >> 8, bgColor & 0xFF};

  uint8_t row_byte = ceil(font->width / 8.0f);

  while (*str != '\0') {
    ST7789_SetAddressWindow(x, y, x + font->width - 1, y + font->height - 1);
    ST7789_WriteCommand(0x2C);

    uint16_t index = (*str - font->ascii_offset) * font->height * row_byte;

    for (row = 0; row < font->height; row++) {
      for (col = 0; col < font->width; col++) {
        if ((font->data[index + col / 8] << (col % 8)) & 0x80) {
          ST7789_WriteData(buffer_color, 2);
        } else {
          ST7789_WriteData(buffer_bg, 2);
        }
      }

      index += row_byte;
    }

    x += font->width;

    str++;
  }
}

void ST7789_Rectangle(uint16_t x, uint16_t y, uint16_t width, uint16_t height, uint16_t color) {
  static uint8_t buff[ST7789_WIDTH * 2] = {0};

  uint16_t i;
  for (i = 0; i < width; i++) {
    buff[i * 2] = color >> 8;
    buff[i * 2 + 1] = color & 0xFF;
  }

  ST7789_SetAddressWindow(x, y, x + width - 1, y + height - 1);
  ST7789_WriteCommand(0x2C);
  // Write the color data
  for (i = 0; i < height; i++) {
    ST7789_WriteData(buff, width * 2);
  }
}

void ST7789_Image(uint16_t x, uint16_t y, uint16_t width, uint16_t height, const uint8_t *image) {
  ST7789_SetAddressWindow(x, y, x + width - 1, y + height - 1);

  ST7789_WriteCommand(0x2C);

  ST7789_WriteData((uint8_t *) image, sizeof(uint16_t) * width * height);
}

void ST7789_FillScreen(uint16_t color) {
  ST7789_Rectangle(0, 0, ST7789_WIDTH, ST7789_HEIGHT, color);
}
